In [ ]:
!pip3 install -r requirements.txt
In [ ]:
Дараах линкээр орж Kalman - ны шүүрийн анхны хэвлэлтэй танилцана уу. </span></div> Kalman filter paper
Бид хүлээгдэж буй $ a $ хурдатгалыг мэдсэнээр тохируулж эсвэл удирдаж болно. Үндсэн kinematic-аас дараах томъёог гаргаж болно.
Ингээд бид хоёр үнэн байх магадлалтай утгатай болсон бөгөөд эдгээрийн аль аль нь үнэн байх магадлалтай учраас тэдгээрийн Гауссын тархалтын хамрах хүрээнүүдийг хооронд нь үржүүлнэ.
Доорхи зурагаар шинэ Гауссын хамрах хүрээг дүрслэв.
Ингээд л боллоо $ \color{royalblue}{\mathbf{\hat{x}}_k'} $ бол бидний хамгийн сайн шинэ тооцоолол ба түүнийг бид цаашид мэдээллээр ( $ \color{royalblue}{\mathbf{P}_k'} $ - ийн хамт) тэжээснээр дараагийн тойрогт эргэж таамаглах болон түүнийг шинэчлэх гэх мэтээр олон дахин давтах ба доорх зурагт дүрслэн үзүүлэв.
Oder well defined Kalman filter iPython example and book
</span></div>
In [2]:
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
from numpy.random import randn
position = np.linspace( 10, 110, 50 ) + randn(50)
velocity = np.linspace( 0, 10, 50 ) + randn(50)
plt.plot(velocity, position,'bo')
plt.grid(True)
print('Mean of readings position is {:.3f}'.format(np.mean(position)))
print('Mean of readings velocity is {:.3f}'.format(np.mean(velocity)))
In [3]:
z_position = np.mean(np.diff(position))
z_velocity = np.mean(np.diff(velocity))
print(np.shape(position), np.shape(velocity))
print(z_position,z_velocity)
In [4]:
mp = np.array((2,1), np.float32) # measurement matrice
tp = np.zeros((2,1), np.float32) # tracked / prediction matrice
print(mp,tp)
In [5]:
def predict(posterior, movement):
x, P = posterior # mean and variance of posterior
dx, Q = movement # mean and variance of movement
x = x + dx
P = P + Q
return x, P
def update(prior, measurement):
x, P = prior # mean and variance of prior
z, R = measurement # mean and variance of measurement
y = z - x # residual
K = P / (P + R) # Kalman gain
x = x + K*y # posterior
P = (1 - K) * P # posterior variance
return x, P
In [ ]:
#Your code here
</span></div>
In [ ]:
#by tugstugi ball bounce video creator
import cv2
import numpy as np
import math
import random
def convert_world_coordinate_to_canvas_coordinate(x, y):
global meter_to_pixel_ratio, blob_radius
x_canvas = int(x*meter_to_pixel_ratio) + blob_radius
y_canvas = height - int(y*meter_to_pixel_ratio) - blob_radius
return (x_canvas, y_canvas)
if __name__ == '__main__':
""""
Creates a video where a ball is moving. Signal noise and false positives are modelled.
"""
noise_sigma = 0.8 # standard deviation for the noise in m
missing_observation_percentage = 10 # how many observations are missing? in percentage
outlier_percentage = 10 # how many observations are false positives? in percentage
start_velocity = 40.0 # start velocity in m/s
start_angle = 70.0 # start angle in degree
dt = 0.033 # time step in second
g = 9.81 # gravitational acceleration in m/s^2
meter_to_pixel_ratio = 5.0 # 1 meter is 5 pixels
blob_radius = 10 # blob radius in pixel
start_velocity_x = start_velocity * math.cos(math.radians(start_angle))
start_velocity_y = start_velocity * math.sin(math.radians(start_angle))
t = 0
width = 640
height = 480
cv2.namedWindow("Simulation")
img = np.zeros((height, width, 3), dtype=np.uint8)
video_writer = cv2.VideoWriter("test.avi", cv2.VideoWriter_fourcc('M','J','P','G'), 30, (width, height), True)
while True:
code = cv2.waitKey(int(dt*1000)) & 0xFF
if code in [27, ord('Q'), ord('q')]:
break
t = t + dt
x = start_velocity_x*t
y = start_velocity_y*t - 0.5*g*t**2
# adding gaussian noise
x_with_noise = random.gauss(x, noise_sigma)
y_with_noise = random.gauss(y, noise_sigma)
if y > 0:
img[:] = 0
# draw the true value?
(x_canvas, y_canvas) = convert_world_coordinate_to_canvas_coordinate(x, y)
# cv2.circle(img, (x_canvas, y_canvas), blob_radius, (0, 255, 0), -1)
r = random.uniform(0, 100)
if t > 4*dt and r < missing_observation_percentage:
# no observation
pass
# elif t > 4*dt and r < missing_observation_percentage + outlier_percentage:
# # adding outlier, some random point
# x_random = int(random.uniform(0, width))
# y_random = int(random.uniform(0, height))
#
# draw outlier
# cv2.circle(img, (x_random, y_random), blob_radius, (255, 255, 255), -1)
else:
# draw noise signal
#print x_canvas, y_canvas
(x_canvas, y_canvas) = convert_world_coordinate_to_canvas_coordinate(x_with_noise, y_with_noise)
cv2.circle(img, (x_canvas, y_canvas), blob_radius, (255, 255, 255), -1)
video_writer.write(img)
else:
break
cv2.imshow("Simulation", img)
cv2.destroyAllWindows()
In [ ]:
import cv2
import numpy as np
import math
def convert_world_coordinate_to_canvas_coordinate(x, y):
global meter_to_pixel_ratio, blob_radius
x_canvas = int(x*meter_to_pixel_ratio) + blob_radius
y_canvas = height - int(y*meter_to_pixel_ratio) - blob_radius
return (x_canvas, y_canvas)
def convert_canvas_coordinate_to_world_coordinate(x_canvas, y_canvas):
global meter_to_pixel_ratio, blob_radius
x = float(x_canvas - blob_radius)/meter_to_pixel_ratio
y = float(height - y_canvas - blob_radius)/meter_to_pixel_ratio
return (x, y)
kalman = cv2.KalmanFilter(6, 2, 0)
dt = 0.033
g = 9.81 # gravitational acceleration in m/s^2
meter_to_pixel_ratio = 5.0 # 1 meter is 5 pixels
blob_radius = 10 # blob radius in pixel
outlier_detection_threshold = 1e-15 # probability threshold for outlier detection
kalman.transitionMatrix = np.array([[ 1.0, 0.0, dt, 0.0, 0.5*dt*dt, 0.0],
[ 0.0, 1.0, 0.0, dt, 0.0, 0.5*dt*dt],
[ 0.0, 0.0, 1.0, 0.0, dt, 0.0],
[ 0.0, 0.0, 0.0, 1.0, 0.0, dt],
[ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], dtype=np.float32)
# [x; y] = measurement_matrix * [x; y; x_velocity; y_velocity; x_acceleration; y_acceleration]
kalman.measurementMatrix =np.array([[ 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float32)
# we have very good system model => process noise covariance matrix has lower values.
# lower values => result will be smoother
kalman.processNoiseCov = np.array([[ 0.001, 0.0, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 0.001, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 0.0, 0.001, 0.0, 0.0, 0.0],
[ 0.0, 0.0, 0.0, 0.001, 0.0, 0.0],
[ 0.0, 0.0, 0.0, 0.0, 0.001, 0.0],
[ 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]], dtype=np.float32)
# if the measurement values have gaussian noise with standard deviation 0.8 => variance = 0.8*0.8
# higher values => result will be smoother
kalman.measurementNoiseCov = np.array([[ 0.64, 0.0],
[ 0.0, 0.64]], dtype=np.float32)
# at start, the error must be high
kalman.errorCovPost = np.array([[ 10.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 10.0, 0.0, 0.0, 0.0, 0.0],
[ 0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
[ 0.0, 0.0, 0.0, 10.0, 0.0, 0.0],
[ 0.0, 0.0, 0.0, 0.0, 10.0, 0.0],
[ 0.0, 0.0, 0.0, 0.0, 0.0, 10.0]], dtype=np.float32)
# initialize the Kalman filter with above matrices.
width = 640
height = 480
cap = cv2.VideoCapture('test.avi')
fourcc = cv2.VideoWriter_fourcc('M','J','P','G')
video_writer = cv2.VideoWriter("Kalman_result1.avi", fourcc, 30, (width, height), True)
P = np.matrix(np.eye(4))*1000 # initial uncertainty
x = np.matrix('0. 0. 0. 0.').T
y = np.matrix('0. 0. 0. 0.').T
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
#gray scale
imgray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
ret,thresh = cv2.threshold(imgray,127,255,0)
_, contours, hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(frame,contours,-1,(0,0,255),-1)
if len(contours)!=0:
cnt = contours[0]
leftmost = cnt[cnt[:,:,0].argmin()][0]
rightmost = cnt[cnt[:,:,0].argmax()][0]
topmost = cnt[cnt[:,:,1].argmin()][0]
bottommost = cnt[cnt[:,:,1].argmax()][0]
# d1 = cv2.sqrt((leftmost[0]-rightmost[0])**2+(leftmost[1]-rightmost[1])**2)
# d2 = cv2.sqrt((topmost[0]-bottommost[0])**2+(topmost[1]-bottommost[1])**2)
# delt=cv2.sqrt((d1[0]-d2[0])**2)
d1 = cv2.norm(leftmost,rightmost,2)
d2 = cv2.norm(topmost,bottommost,2)
delt = cv2.norm(d1,d2,2)
if delt <= 2:
M = cv2.moments(cnt)
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
measurement = np.array((2,1), np.float32)
measurement[0] = cx
measurement[1] = cy
kalman.predict()
kalman.correct(measurement)
(x, y, x_velocity, y_velocity) = (kalman.statePost[0, 0], kalman.statePost[1, 0], kalman.statePost[2, 0], kalman.statePost[3, 0])
# draw hypothesis as green circle
orientation = math.atan2(y_velocity, x_velocity)
length = math.sqrt(x_velocity**2+y_velocity**2)*0.5/10
cv2.circle(frame, (x, y), 10, (0, 255, 0), 2)
#print frame.shape
print (x_velocity, y_velocity)
(x_velocity_canvas, y_velocity_canvas) = convert_world_coordinate_to_canvas_coordinate(x + length*math.cos(orientation), y + length*math.sin(orientation))
x_velocity_canvas, y_velocity_canvas = int(x_velocity_canvas/100), int(y_velocity_canvas/100)
#print (x_velocity_canvas, y_velocity_canvas)
cv2.arrowedLine(frame, (x, y), (int(x+abs(x_velocity_canvas)), int(y+abs(y_velocity_canvas))), (0, 255, 0), 1)
cv2.putText(frame,"V = %d %.2fdeg" %(length, orientation), (int(x+10),y), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255))
#print (int(measurement[0]),int(measurement[1]))
#cv2.circle()
cv2.imshow("frame", frame)
video_writer.write(frame)
cv2.waitKey(33)
cap.release()
cv2.destroyAllWindows()
In [ ]: